#include "Primitive.h"
#include "ContactManifold.h"
#include <Windows.h>
#include "Circle.h"
#include "Line.h"
#include <iostream>


float Primitive::Elasticity = 0.8f;
float Primitive::Friction = 0.002f;
Game* Primitive::m_game = (Game*)0;


Primitive::Primitive() : m_velocity(Vector(0,0,0)), m_angularVel(Vector(0,0,0)), 
						 m_inertia(0.1f), m_invMass(0.0f), m_invInertia(0.0f) {}

Primitive::Primitive(Vector vel, Vector angVel, float mass, float inertia, bool active) : m_velocity(vel), 
							m_angularVel(angVel), m_inertia(inertia)
{ }


float	Primitive::toDegrees(float rad) const
{
	return rad*(180.0f/3.14f);
}


void	Primitive::CollisionResponse(ManifoldPoint& point, float dt)
{
	Primitive* c0 = point.contactID2;
	Primitive* c11 = point.contactID1;

	

	float invDt = 1.0f / dt;

	float dist = point.contactPoint.GetZ();
	point.contactPoint.SetZ(0.0f);

	Vector nn = point.contactNormal;
	
	Vector r1 = c0->vectorTo(point.contactPoint);
	Vector r2 = c11->vectorTo(point.contactPoint);
	r1.SetZ(0);
	r2.SetZ(0);

	// get all of relative normal velocity
	double relNv = (c11->GetVel()				  -		c0->GetVel()  +  
					c11->GetAngularVel().cross(r2) - c0->GetAngularVel().cross(r1)).dot(nn);

	float vBias = 0.0f;
	if (point.t == -1.0f) {
		vBias = (dist-0.0001f) * 0.2f * invDt;
	}

	double velBias = relNv * Elasticity;
	double remove = relNv - vBias ;

	if (dynamic_cast<Line*>(c0) || dynamic_cast<Line*>(c11))
	{
		remove += velBias;
	}
	else
	{
		remove -= velBias;
	}
	
	double denom = c0->invMass() + c11->invMass();

	denom += 
			point.contactNormal.dot(Vector((r1.cross(point.contactNormal)) *  c0->invInertia()).cross(r1)) +
			point.contactNormal.dot(Vector((r2.cross(point.contactNormal)) * c11->invInertia()).cross(r2)); 
	denom = 1.0 / denom;

	//// compute impulse
	double imp = remove * denom; 
	 
	// sequential impulses
	double temp = point.impulse;
	point.impulse = min(point.impulse + imp, 0);
	imp = point.impulse - temp;

	//////////////

	// apply impulse
	Vector Vel0 = c0->GetVel()  +  (float)imp * nn  * c0->invMass();
	Vector Vel1 = c11->GetVel() -  (float)imp * nn  * c11->invMass();

	c11->SetVel(Vel1.GetX(), Vel1.GetY());
	c0->SetVel(Vel0.GetX(), Vel0.GetY());

	point.responded = true;

	Vector angVel0 = c0->GetAngularVel() + (r1.cross((float)imp*nn)) * c0->invInertia();
	Vector angVel1 = c11->GetAngularVel() - (r2.cross((float)imp*nn)) * c11->invInertia();
	
	c0->SetAngularVel(angVel0);
	c11->SetAngularVel(angVel1);
	
	// ----------------------------------
	// F R I C T I O N ------------------
	// __________________________________
	bool bFriction = true;

	if (bFriction) {
		Vector t = nn.cross(Vector(0,0,1));

		denom = 
			(c0->invMass() + c11->invMass() + 
		t.dot(Vector((r1.cross(t)) *  c0->invInertia()).cross(r1)) +
		t.dot(Vector((r2.cross(t)) * c11->invInertia()).cross(r2))); 
	}
	denom = 1.0 / denom;

	Vector t = nn.cross(Vector(0,0,1));
	double vt = (c11->GetVel()				  -		c0->GetVel()  +  
					c11->GetAngularVel().cross(r2) - c0->GetAngularVel().cross(r1)).dot(t);
	double dPt = denom * (-vt);

	// sequential impulse
	double maxPt = Friction * point.impulse;

	double oldTangentImpulse = point.t_impulse;
	// clamp
	point.t_impulse = oldTangentImpulse + dPt;
	if (point.t_impulse < -maxPt)
		point.t_impulse = -maxPt;
	else if (point.t_impulse > maxPt)
		point.t_impulse = maxPt;

	dPt = point.t_impulse - oldTangentImpulse;
	 // /

	// Apply contact impulse
	Vector Pt = (float)dPt * t;

	Vel0 = c0->GetVel() + Pt * c0->invMass();
	Vel1 = c11->GetVel() + Pt * c11->invMass(); 
		
	Vel0 = c0->GetVel() + Pt * c0->invMass();
	Vel1 = c11->GetVel() - Pt * c11->invMass(); 

	c0->SetVel(Vel0.GetX(), Vel0.GetY());
	c11->SetVel(Vel1.GetX(), Vel1.GetY());
	
	
	angVel0 = c0->GetAngularVel() + (r1.cross(Pt))  * c0->invInertia();
	angVel1 = c11->GetAngularVel() - (r2.cross(Pt)) * c11->invInertia();
	
	c0->SetAngularVel(angVel0);
	c11->SetAngularVel(angVel1);
	

	point.responded = true;
}


Vector	Primitive::GetAngularVel() const
{
	return m_angularVel;
}
	
	
float	Primitive::GetInertia()
{
	return m_inertia;
}



float	Primitive::GetMass() const
{
	return m_data->m_mass;
}

Vector	Primitive::GetVel() const
{
	return m_velocity;
}

void	Primitive::SetVel(float x, float y)
{
	m_velocity.Set(x, y);
}

void	Primitive::SetMass(float mass)
{
	m_data->m_mass = mass;
}



Vector	Primitive::GetAccel() const
{
	return m_accel;
}

void	Primitive::SetPos(float x, float y)
{
	#ifdef CATCH_NAN	
		if (isNaN(Vector(x,y))) {
			std::cout << "NaN @ Circle::SetPos" << std::endl;
		}
	#endif

	m_data->m_pos.Set(x,y);
}



void	Primitive::setActive(bool v)
{
	m_data->m_bActive = v;
}

bool	Primitive::getActive()
{
	return m_data->m_bActive;
}


Vector Primitive::GetPos() const
{
	return m_data->m_pos;
}
